import rclpy
from rclpy.node import Node
from .pca9685 import Steers
import threading
from topic_msg.msg import CatSteer

class CatIicNode(Node):
    def __init__(self, name):
        super().__init__(name)
        self._iic_lock = threading.Lock()
        self._steers = Steers()
        self._steer_subscriper = self.create_subscription(
            CatSteer, "cat_steers", self._set_group_steer, 10
        )

    def __del__(self):
        self._steer_subscriper.destroy()

    def _set_group_steer(self, msg):
        with self._iic_lock:
            self._steers.set_group_servo(msg)


def main():
    rclpy.init()
    node = CatIicNode("cat_steer")
    rclpy.spin(node)
    rclpy.shutdown()
